'''
扫把倒立平衡（全方向倒立摆）程序

本程序适用于程欢欢智能小车（机器人）。
移植需要修改，但所有内容均开源。
OpenMV的识别库，可以参考OpenMV的GitHub。
我的car库（底盘驱动），开源本文所用到的驱动及获取编码器的部分。更多car库驱不开源。

对于使用程欢欢智能小车（机器人），需要修改(调试)的内容：
sensor.set_windowing(80,70,128,96) -- 切割画面范围
threshold = (40, 100, 54, 85, -15, 66) -- 目标阈值，可以用OpenMV的IDE的工具获取。
target_original = (70,61) -- 目标坐标：保持直立平衡时，标签所在的坐标。不一定要绝对准确，程序可以自行调整。
countdown_times = 200 -- 丢失目标后停止程序，初步调参时可以改为无限大
... -- 所有PID参数

调节PID参数时，可以打印对应PID的：
***.p
***.i
***.d
来查看每个环节的执行效果。这是原PID库作者没有写入，我加入的部分。这很有助于调节参数。
所以pid库改名为ppid，避免和OpenMV内置的pid库混淆。
关于PID的原理，可以查看我的B站视频。包含可以写出此PID库的完整知识。

对于移植需要修改的内容：
调节上述所有参数
删除wifi图传相关
删除屏幕显示相关
自行建立麦克纳姆轮驱动

注意：
需要在外界环境亮度足够的前提下。
使用OV7725及其他高速摄像头，可以达到约150Hz帧速。
OpenMV4Plus标配的OV5640是无法达到的。
另外注意开始运行程序时补光灯不要进入摄像头视野（包括被剪裁掉的部分），\
否则会影响摄像头初始化的亮度设置。
如果没有补光灯，可以使用自发光球，顶在杆上面。这也是我最早的方案。也许我以后会再出一期发光球方案的视频。
建议不要使用红色胶带，手和脸容易被误识别。我拍摄视频时没有其他颜色可用，但因此NG了很多次。
***扫把太轻，平衡难度很大。换上发光球（比较重的），效果会好很多！***
别忘关注作者。

B站、抖音
@程欢欢的智能控制集
20230317
'''
import sensor, image, time, car, ppid, esp32    #导入所需的库
from pyb import millis

###声明各个硬件
screen = car.screen() #声明屏幕。因为部分设置可能和摄像头冲突，所以在摄像头之前声明（初始化）。
screen.high_speed_mode(2)   #屏幕高速模式2，因为使用DMA三重缓冲，所以限制画幅。可用显示区160*120。
gamepad = car.gamepad()   #声明遥控手柄
drive = car.drive() #声明小车驱动
drive_close_loop = car.drive_close_loop()   #声明闭环驱动，仅用于读取电机编码器
drive_close_loop.lock_degrees(False)    #关闭闭环驱动对电机的角度锁。

###声明WiFi图传模块。
#注：将小车的WiFi模块连接至热点（路由），会更稳定。在设置中连接完成后，此处会按照
#   上一次的设置初始化wifi模块。
wifi = esp32.wifi_video_stream()  #声明ESP32的wifi类
wifi_refresh_timer = millis()  #wifi刷新时间所用的临时变量
wifi_refresh_time = 40  #设置wifi刷新时间，单位ms

###声明摄像头
sensor.reset()  #摄像头重启
sensor.set_pixformat(sensor.RGB565) #16位彩色模式
sensor.set_framesize(sensor.QVGA)   #320*240分辨率
#切割其中128*96的画幅使用。前两个坐标是画面起点，需要根据实际硬件调整。
sensor.set_windowing(80,70,128,96)
sensor.set_auto_whitebal(False)  #关闭自动白平衡
sensor.set_auto_gain(False) #关闭自动亮度
#sensor.set_auto_exposure(False,exposure_us=500)    #强制设定曝光时间，暂不启用
sensor.set_contrast(3)  #设置对比度，范围-3到3。
sensor.set_brightness(0)   #设置亮度，范围-3到3.
sensor.set_saturation(3)    #设置饱和度，范围-3到3，
clock = time.clock()    #声明clock，用于主循环中统计帧速。

###变量
countdown_times = 200   #计数：当连续*次标签与目标没有重叠，即停止程序。防止目标丢失后乱跑。
countdown = 0   #计数所用临时变量。
threshold = (20, 100, 35, 85, -20, 70)#红胶带阈值，可以用OpenMV的IDE获取。
target_original = (70,58)   #目标坐标：保持直立平衡时，标签所在的坐标。不一定要绝对准确，程序可以自行调整。
target = [70,58]    #目标坐标临时变量，初始值与上面相同即可。
remote_x = 0    #遥控所用临时变量
remote_y = 0
fps = 0 #帧速所用临时变量

#PID参数。共三个环，每个环分为X、Y坐标。
pid_angle_x=ppid.PID(p=10,i=30,d=0.13,imax=80)   #扫把倾角输出小车移动环
pid_angle_y=ppid.PID(p=10,i=30,d=0.13,imax=80)
pid_distance_to_target_x=ppid.PID(p=5,i=0,d=0.2)    #行进距离输出目标修正环
pid_distance_to_target_y=ppid.PID(p=5,i=0,d=0.2)
pid_distance_to_move_x=ppid.PID(p=4,i=0,d=0.1)  #行进距离输出小车移动环
pid_distance_to_move_y=ppid.PID(p=4,i=0,d=0.1)

###等待扫把标签与目标点重叠，再执行后续程序
while True:
    img = sensor.snapshot() #获取图像
    blobs = img.find_blobs([threshold], x_stride=4, y_stride=4) #按阈值找标签
    #画十字线，标示目标
    img.draw_line(0, round(target[1]), 320, round(target[1]),color=(150,150,150))
    img.draw_line(round(target[0]), 0, round(target[0]), 240,color=(150,150,150))
    if blobs:   #如果找到标签
        blob = max(blobs, key = lambda b: b[4]) #获取面积最大的标签
        img.draw_rectangle(blob[0:4],color=(0,0,255))   #画方块标示标签
        img.draw_cross(blob.cx(), blob.cy(),color=(0,0,255))    #画十字标示标签
        if abs(blob.cx() - target[0])<3 and abs(blob.cy() - target[1])<3:   #标签与目标相距3个像素以内
            break   #跳出循环
    screen.display(img) #屏幕显示
    if millis() - wifi_refresh_timer > wifi_refresh_time:
        wifi_refresh_timer = millis()
        wifi.display(img,quality=80)   #使wifi模块显示图像

###主循环，负责自平衡、遥控、屏幕显示、WiFi图传
while True:
    clock.tick()    #获取帧速所需的语句，需要在循环开始执行
    img = sensor.snapshot() #获取图像
    blobs = img.find_blobs([threshold], x_stride=4, y_stride=4) #按阈值找标签
    ##画十字线，标示目标
    img.draw_line(0, round(target[1]), 320, round(target[1]),color=(150,150,150))
    img.draw_line(round(target[0]), 0, round(target[0]), 240,color=(150,150,150))
    ##找到标签
    if blobs:   #如果找到标签
        blob = max(blobs, key = lambda b: b[4])#获取面积最大的标签
        if abs(blob.cx() - target[0])<5 and abs(blob.cy() - target[1])<5: #标签与目标相距5个像素以内
            countdown = 0   #计数归零
        else:   #否则
            countdown += 1  #计数自加
        img.draw_rectangle(blob[0:4],color=(0,0,255))   #画方块标示标签
        img.draw_cross(blob.cx(), blob.cy(),color=(0,0,255))    #画十字标示标签
        ##获取四个电机的编码器值
        m0 = drive_close_loop.get_motor_degrees(0)  #左前轮
        m1 = drive_close_loop.get_motor_degrees(1)  #左后轮
        m2 = drive_close_loop.get_motor_degrees(2)  #右前轮
        m3 = drive_close_loop.get_motor_degrees(3)  #右后轮
        ##正运算，计算整车行进距离。这个量会作为补偿量参与计算，所以遥控量在此放入。
        distance_x = m0 - m1 - m2 + m3 + remote_x   #整车X轴行进距离，加遥控量
        distance_y = m0 + m1 + m2 + m3 + remote_y   #整车Y轴行进距离，加遥控量
        ##遥控量自加。*0.03缩小遥控量。
        remote_x -= gamepad.L_X() * 0.03
        remote_y -= gamepad.L_Y() * 0.03
        ##行进距离输出目标修正环。PID内的0.001.是对输出量的缩放。
        target[0] = target_original[0] + pid_distance_to_target_x.get_pid(distance_x, 0.001)
        target[1] = target_original[1] + pid_distance_to_target_y.get_pid(distance_y, 0.001)
        ##扫把倾角输出小车移动环 和 行进距离输出小车移动环。
        x_move = pid_angle_x.get_pid(target[0] - blob.cx(), 1) \
                    + pid_distance_to_move_x.get_pid(distance_x, 0.02)
        y_move = pid_angle_y.get_pid(target[1] - blob.cy(), 1) \
                    + pid_distance_to_move_y.get_pid(distance_y, 0.02)
        ##小车移动。遥控*0.2限制幅度。
        drive.move(x_move, y_move, gamepad.R_X() * 0.2)
        ##在图像上绘制小车前后、左右的移动量标示条
        if x_move>0:
            img.draw_rectangle(round(target[0]),5,round(x_move*0.8),10,fill=True,color=(0,255,0))
        else:
            img.draw_rectangle(round(target[0])+round(x_move*0.8),5,round(-x_move*0.8),10,fill=True,color=(255,0,0))
        if y_move>0:
            img.draw_rectangle(5,round(target[1]),10,round(y_move*0.8),fill=True,color=(255,0,0))
        else:
            img.draw_rectangle(5,round(target[1])+round(y_move*0.8),10,round(-y_move*0.8),10,fill=True,color=(0,255,0))
    else:   #没有找到标签
        countdown += 1  #计数自加
        drive.move(0,0,0)   #小车停止
    ##计数达到阈值，停止程序
    if countdown >= countdown_times:
        break
    img.draw_string(2,2,'FPS:'+str(round(fps, 2) ),x_spacing=-3 )#绘制帧速到图像
    ##达到刷新时间，WiFi图传刷新图像
    if millis() - wifi_refresh_timer > wifi_refresh_time:
        wifi_refresh_timer = millis()
        wifi.display(img,quality=80)   #使wifi模块显示图像
    screen.display(img) #屏幕显示
    fps = clock.fps()   #获取帧速，与clock.tick() 相呼应。这条需要放在循环末尾。
